#include "../DMC_LIB/DMC.h"

#include <math.h>
#include <string.h>



/*---------------------------------------------------------------------MidData---------------------------------------------------------------------*/
static float32 	sg_fK1 = ACC * T_PWM / BASE_RPM;
static int32 	sg_lK2 = RDC_SCALER * RSL_POLE_PAIRS / MOTOR_POLE_PAIRS;
static Uint16 	sg_uiK3 = MOTOR_POLE_PAIRS / RSL_POLE_PAIRS;
static int32 	sg_lK4 = RDC_SCALER / 2;
static float32 	sg_fK5 = (float32)F_PWM * 60.f / RDC_SCALER / RSL_POLE_PAIRS / 128 / BASE_RPM;
static float32 	sg_fK6 = BASE_RPM / 60.F * MOTOR_POLE_PAIRS * T_PWM;

static Uint16 sg_uiRslOffsetPos = RSL_OFFSET_POS;
static Uint16 sg_uiRslOffsetNeg = RSL_OFFSET_NEG;

float32 g_Kp = KP;
float32 g_Ki = KI;


/*---------------------------------------------------------------------POSTION---------------------------------------------------------------------*/

/*-------------------------------------------------------------------------------
 * Function 	>	PostionCL
 * Description 	>	Positoin close loop control.
 * Input 		>	fPosRef			:	Target value (per unit)
 					fPosFb			:	FeedBack value (per unit)
 * Output 		>	fSpdRef			:	Speed target value (per unit)
 *-------------------------------------------------------------------------------*/
float32 PostionCL(float32 fPosRef , float32 fPosFb)
{
	int16 iLimit = 0;
	float32 fSpdRef = 0.F;
	float32 fDelta = 0.F;

	fPosRef = LimitF(fPosRef, 1.F, -1.F);
	fDelta = fPosRef - fPosFb;

	iLimit = HysteresisCtrl(fDelta);
	if( iLimit == 1 )
	{
		if(fDelta > 0.F)
		{
			fSpdRef = (fDelta - DELTA_P3) * K_P + V_MIN;
			if( fSpdRef < V_MIN )
			{
				fSpdRef = V_MIN;
			}
		}
		else
		{
			fSpdRef = (fDelta + DELTA_P3) * K_P - V_MIN;
			if( fSpdRef > -V_MIN )
			{
				fSpdRef = -V_MIN;
			}
		}
	}
	else
	{
		fSpdRef = 0.F;
	}
	fSpdRef = LimitF(fSpdRef, 1.F, -1.F);
	return fSpdRef;
}

/*-------------------------------------------------------------------------------
 * Function 	>	HysteresisCtrl
 * Description 	>	Hysteresis loop control.
 * Input 		>	fValue			:	Hysteresis input data
 * Output 		>	s_uiRtnCofft	:	Hysteresis Output data
 *-------------------------------------------------------------------------------*/
int16 HysteresisCtrl(float32 fValue)
{
	static Uint16 s_uiRtnCofft = 0;
	if(s_uiRtnCofft == 1)
	{
		if( (fValue <= DELTA_P1) && (fValue >= -DELTA_P1) )
		{
			s_uiRtnCofft = 0;
		}
	}
	else
	{
		if( (fValue <= -DELTA_P2) || (fValue >= DELTA_P2) )
		{
			s_uiRtnCofft = 1;
		}
	}
	return s_uiRtnCofft;
}


/*---------------------------------------------------------------------SPEED---------------------------------------------------------------------*/
/*-------------------------------------------------------------------------------
 * Function 	>	DmcSpeedCL
 * Description 	>	speed close loop control
 * Input 		>	1.uiEn		2.fSpdRef		3.uiRdcRaw
 * Output 		>	1.fSpdFb	2.fSpdRefRamp	3.fUthe		4.fUamp		5.fTa	6.fTb	7.fTc
 *-------------------------------------------------------------------------------*/
void DmcSpeedCL(struct DMC_DATA *v)
{
	v->fSpdFb = SpeedFeed(v->uiRdcRaw);

	v->fSpdRefRamp = SpeedRamp(v->uiEn, v->fSpdRef, v->fSpdRefRamp);

	v->fUamp = SpeedPI(v->uiEn, v->fSpdRefRamp, v->fSpdFb);

	v->fUthe = GetThetaE(v->uiRdcRaw , v->fUamp);

	SPWMZ_Gen(v->fUamp , v->fUthe , &(v->fTa) , &(v->fTb) , &(v->fTc));
}

/*-------------------------------------------------------------------------------
 * Function 	>	SpeedRamp
 * Description 	>	Ramp Step to fSpdRef with "sg_fK1" every T_PWM.
 * Input 		>	uiEn				:	1=Enable;	other=Disable
					fSpdRef				:	Target value (per unit)
 	 	 	 	 	fSpdRefRamp			:	last step value (per unit)
 * Output 		>	fSpdRefRamp			:	new step value (per unit)
 *-------------------------------------------------------------------------------*/
//#pragma CODE_SECTION(SpeedRamp,"ramfuncs");
float32 SpeedRamp( Uint16 uiEn, float32 fSpdRef, float32 fSpdRefRamp)
{
	if(uiEn == 1)
	{
		if(fSpdRefRamp < fSpdRef)
		{
			fSpdRefRamp = ( (fSpdRefRamp + sg_fK1) > fSpdRef ) ? fSpdRef : (fSpdRefRamp + sg_fK1);
		}
		else
		{
			fSpdRefRamp = ( (fSpdRefRamp - sg_fK1) < fSpdRef ) ? fSpdRef : (fSpdRefRamp - sg_fK1);
		}
		fSpdRefRamp= LimitF(fSpdRefRamp, 1.F, -1.F);
	}
	else
	{
		fSpdRefRamp = 0;
	}
	return fSpdRefRamp;
}

/*-------------------------------------------------------------------------------
 * Function 	>	SpeedFeed
 * Description 	>	rdcRaw to motor speed per unit,with 128 period slip average filter.
 * Input 		>	uiRdcRaw		:	raw data from rdc
 * Output 		>	fSpdFb			:	Feed speed (per unit)
 *-------------------------------------------------------------------------------*/
//#pragma CODE_SECTION(SpeedFeed,"ramfuncs");
float32 SpeedFeed(Uint16 uiRdcRaw)
{
	float32 fSpdFb = 0.F;
	int32 lErr = 0L;
	static Uint16 s_uiRdcRawLast = 0L;
	static Uint16 s_uiIndex = 0U;
	static int32 s_lErrSum = 0L;
	static int32 s_lErrQueue[128] = {
									0L, 0L, 0L, 0L, 0L, 0L, 0L, 0L,0L, 0L, 0L, 0L, 0L, 0L, 0L, 0L,
									0L, 0L, 0L, 0L, 0L, 0L, 0L, 0L,0L, 0L, 0L, 0L, 0L, 0L, 0L, 0L,
									0L, 0L, 0L, 0L, 0L, 0L, 0L, 0L,0L, 0L, 0L, 0L, 0L, 0L, 0L, 0L,
									0L, 0L, 0L, 0L, 0L, 0L, 0L, 0L,0L, 0L, 0L, 0L, 0L, 0L, 0L, 0L,
									0L, 0L, 0L, 0L, 0L, 0L, 0L, 0L,0L, 0L, 0L, 0L, 0L, 0L, 0L, 0L,
									0L, 0L, 0L, 0L, 0L, 0L, 0L, 0L,0L, 0L, 0L, 0L, 0L, 0L, 0L, 0L,
									0L, 0L, 0L, 0L, 0L, 0L, 0L, 0L,0L, 0L, 0L, 0L, 0L, 0L, 0L, 0L,
									0L, 0L, 0L, 0L, 0L, 0L, 0L, 0L,0L, 0L, 0L, 0L, 0L, 0L, 0L, 0L,
								};

	//get err and round roll
	lErr = (int32)uiRdcRaw - (int32)s_uiRdcRawLast;
	lErr = (lErr >  sg_lK4) ? (lErr - RDC_SCALER) : lErr;
	lErr = (lErr < -sg_lK4) ? (lErr + RDC_SCALER) : lErr;
	s_uiRdcRawLast = uiRdcRaw;

	//roll errQueue, newest in and oldest out.
	s_lErrSum = s_lErrSum - s_lErrQueue[s_uiIndex] + lErr;
	s_lErrQueue[s_uiIndex] = lErr;
	s_uiIndex = (s_uiIndex + 1) % 128;

	//compute Feed_pu
	fSpdFb = s_lErrSum * sg_fK5;
	return fSpdFb;
}


/*-------------------------------------------------------------------------------
 * Function 	>	SpeedPI
 * Description 	>	speed front + p + i control.
 * Input 		>	uiEn				:	1=Enable;	other=Disable
					fSpdRef				:	Target value (per unit)
 	 	 	 	 	fSpdFb				:	FeedBack value (per unit)
 * Output 		>	fUamp				:	Motor stator voltage output (per unit)
 *-------------------------------------------------------------------------------*/
//#pragma CODE_SECTION(SpeedPI,"ramfuncs");
float32 SpeedPI( Uint16 uiEn, float32 fSpdRef, float32 fSpdFb)
{
	float32 fUp = 0.F;				// Data: proportional term
	static float32 s_fUi = 0.F;		// Data: integral term
	float32 fUf = 0.F;				// Data: front term
	float32 fUamp = 0.F;			// Data: output term

	if(uiEn == 1)
	{
		/* front term */
		fUf = KF * fSpdRef;

		/* proportional term */
		fUp = g_Kp * (fSpdRef - fSpdFb);
		fUamp = LimitF( (fUp + fUf), ULIM1, DLIM1);

		/* integral term */
		s_fUi = (g_Ki * fUp) + s_fUi;
		s_fUi = LimitF(s_fUi, (ULIM3 - fabs(fUamp)), (DLIM3 + fabs(fUamp)) );

		/* control output */
		fUamp = LimitF( (fUamp + s_fUi), ULIM3, DLIM3);
	}
	else
	{
		s_fUi = 0.F;
		fUamp = 0.F;
	}
	return fUamp;
}




/*---------------------------------------------------------------------PWM---------------------------------------------------------------------*/
/*-------------------------------------------------------------------------------
 * Function 	>	GetThetaE
 * Description 	>	Convert Rdc raw data to Electrical Angle with offset correct
 * Input 		>	uiRdcRaw			:	raw data from rdc
					fUamp				:	Motor stator voltage output (per unit)
 * Output 		>	fUthe				:	Motor stator Electrical Angle (per unit)
 *-------------------------------------------------------------------------------*/
//#pragma CODE_SECTION(GetTheta,"ramfuncs");
float32 GetThetaE(Uint16 uiRdcRaw, float32 fUamp)
{
	Uint16 uiOffset = (fUamp > 0) ? sg_uiRslOffsetPos : sg_uiRslOffsetNeg;
	return ( (uiRdcRaw % sg_lK2) * sg_uiK3 + uiOffset )  % RDC_SCALER / (float32)RDC_SCALER;
}

/*-------------------------------------------------------------------------------
 * Function 	>	SPWMZ_Gen
 * Description 	>	Sin PWM with Zero-Sequence Voltage Injection
 * Input 		>	fUamp				:	Motor stator volt amplitude (per unit)
					fTheta				:	Motor stator Electrical Angle (per unit)
 * Output 		>	fTa					:	PWM-A
					fTb					:	PWM-B
					fTc					:	PWM-C
 *-------------------------------------------------------------------------------*/
//#pragma CODE_SECTION(SPWMZ_Gen,"ramfuncs");

void SPWMZ_Gen(float32 fUamp, float32 fTheta, float32 *fTa, float32 *fTb, float32 *fTc)
{
	float32 fUa = 0.F;
	float32 fUb = 0.F;
	float32 fUc = 0.F;
	float32 fMax = 0.F;
	float32 fMin = 0.F;
	float32 fVCM = 0.F; 			//common-mode voltages

	fUa = sin(fTheta * 6.28318530718F) * fUamp / 2.F;					//6.28318530718F = 2*pi
	fUb = sin(fTheta * 6.28318530718F + 2.0943951023932F) * fUamp / 2.F;//2.0943951023932F = 2*pi/3
	fUc = 0.F - (fUa + fUb);

	fMax = (fUa  > fUb) ? fUa  : fUb;
	fMax = (fMax > fUc) ? fMax : fUc;
	fMin = (fUa  < fUb) ? fUa  : fUb;
	fMin = (fMin < fUc) ? fMin : fUc;
	fVCM = ((fMax + fMin) / 2);

	*fTa = 0.5 + (fUa - fVCM) * 1.15470F;//1.15470F = 2/SQRT(3)
	*fTb = 0.5 + (fUb - fVCM) * 1.15470F;
	*fTc = 0.5 + (fUc - fVCM) * 1.15470F;

	*fTa = LimitF( *fTa, 1.F, 0.F);
	*fTb = LimitF( *fTb, 1.F, 0.F);
	*fTc = LimitF( *fTc, 1.F, 0.F);
}

/*-------------------------------------------------------------------------------
 * Function 	>	SVPWM_Gen
 * Description 	>	Space vector PWM
 * Input 		>	fUalpha				:	Motor stator volt Ualpha (per unit)
					fUbeta				:	Motor stator volt Ubeta (per unit)
 * Output 		>	fTa					:	PWM-A
					fTb					:	PWM-B
					fTc					:	PWM-C
 *-------------------------------------------------------------------------------*/
//#pragma CODE_SECTION(SVPWM_Gen,"ramfuncs");
void SVPWM_Gen(float32 fUalpha, float32 fUbeta, float32 *fTa, float32 *fTb, float32 *fTc)
{
	Uint16 uiSector = 0U;
	float32 fTemp_sv1 = 0.F;
	float32 fTemp_sv2 = 0.F;
	float32 fVa = 0.F;
	float32 fVb = 0.F;
	float32 fVc = 0.F;
	float32 fT1 = 0.F;
	float32 fT2 = 0.F;

	fTemp_sv1 = fUbeta / 2; 					/*divide by 2*/
	fTemp_sv2 = 0.8660254 * fUalpha;	/* 0.8660254 = sqrt(3)/2*/

/* Inverse clarke transformation */
	fVa = fUbeta;
	fVb = -fTemp_sv1 + fTemp_sv2;
	fVc = -fTemp_sv1 - fTemp_sv2;
/* 60 degree Sector determination */
	if (fVa > 0) uiSector = 1;
	if (fVb > 0) uiSector = uiSector + 2;
	if (fVc > 0) uiSector = uiSector + 4;
/* X,Y,Z (Va,Vb,Vc) calculations X = Va, Y = Vb, Z = Vc */
	fVa = fUbeta;
	fVb = fTemp_sv1 + fTemp_sv2;
	fVc = fTemp_sv1 - fTemp_sv2;
/* Sector 0: this is special case for (Ualpha,Ubeta) = (0,0)*/

	switch(uiSector)
	{
		case 0:
			*fTa = 0.5;
			*fTb = 0.5;
			*fTc = 0.5;
			break;
		case 1:   /*Sector 1: t1=Z and t2=Y (abc ---> Tb,Ta,Tc)*/
			fT1 = fVc;
			fT2 = fVb;
			*fTb = (1 - fT1 - fT2) / 2;
			*fTa = *fTb + fT1;			 	/* taon = tbon+t1		*/
			*fTc = *fTa + fT2;			  	/* tcon = taon+t2		*/
			break;
		case 2:	  /* Sector 2: t1=Y and t2=-X (abc ---> Ta,Tc,Tb)*/
			fT1 = fVb;
			fT2 = -fVa;
			*fTa = (1 - fT1 - fT2) / 2;
			*fTc = *fTa + fT1;				/*  tcon = taon+t1		*/
			*fTb = *fTc + fT2;				/*  tbon = tcon+t2		*/
			break;
		case 3:	  /* Sector 3: t1=-Z and t2=X (abc ---> Ta,Tb,Tc)*/
			fT1 = -fVc;
			fT2 = fVa;
			*fTa = (1 - fT1 - fT2) / 2;
			*fTb = *fTa + fT1;				/*	tbon = taon+t1		*/
			*fTc = *fTb + fT2;				/*	tcon = tbon+t2		*/
			break;
		case 4:	  /* Sector 4: t1=-X and t2=Z (abc ---> Tc,Tb,Ta)*/
			fT1 = -fVa;
			fT2 = fVc;
			*fTc = (1 - fT1 - fT2) / 2;
			*fTb = *fTc + fT1;				/*	tbon = tcon+t1		*/
			*fTa = *fTb + fT2;				/*	taon = tbon+t2		*/
			break;
		case 5:	  /* Sector 5: t1=X and t2=-Y (abc ---> Tb,Tc,Ta)*/
			fT1 = fVa;
			fT2 = -fVb;					/*	tbon = (1-t1-t2)/2	*/
			*fTb = (1 - fT1 - fT2) / 2;
			*fTc = *fTb + fT1;				/*	taon = tcon+t2		*/
			*fTa = *fTc + fT2;
			break;
		case 6:	  /* Sector 6: t1=-Y and t2=-Z (abc ---> Tc,Ta,Tb)*/
			fT1 = -fVb;
			fT2 = -fVc;
			*fTc = (1 - fT1 - fT2) / 2;
			*fTa = *fTc + fT1;				/*	taon = tcon+t1		*/
			*fTb = *fTa + fT2;				/*	tbon = taon+t2		*/
			break;
	}
	/* Ta,Tb,Tc (ranged (0,1)) ->.. */
	return ;
}

/*---------------------------------------------------------------------COMMON---------------------------------------------------------------------*/


/*-------------------------------------------------------------------------------
 * Function 	>	LimitF
 * Description 	>	Limit fVal
 * Input 		>	fVal
          	   	 	fMax
  		   	   	 	fMin
 * Output 		>	fVal
 *-------------------------------------------------------------------------------*/
//#pragma CODE_SECTION(LimitF, "ramfuncs");
float32 LimitF(float32 fVal, const float32 fMax, const float32 fMin)
{
    if(fVal > fMax)   /*over top*/
    {
        fVal = fMax;
    }
    else if(fVal < fMin)  /*over bottom*/
    {
        fVal = fMin;
    }
    else
    {;}

    return fVal;
}

/*-------------------------------------------------------------------------------
 * Function 	>	IClark
 * Description 	>	Inverse CLARK Transformation
 * Input 		>	fAlpha
          	   	 	fBeta
 * Output 		>	fAs
  				 	fBs
  		 		 	fCs
 *-------------------------------------------------------------------------------*/
//#pragma CODE_SECTION(IClark,"ramfuncs");
void IClark(float32 fAlpha, float32 fBeta, float32 *fAs, float32 *fBs, float32 *fCs)
{
	*fAs = fAlpha;
	*fBs = (1.73205F * fBeta - fAlpha) / 2;		// 1.73205 = sqrt(3)
	*fCs = 0 - *fAs - *fBs;
	return;
}

void Clark(float32 fAs, float32 fBs, float32 fCs, float32 *fAlpha, float32 *fBeta)
{
	*fAlpha = fAs;
	*fBeta = (fBs - fCs) * 0.57735026918963;		// 1/sqrt(3) = 0.57735026918963
	return;
}

/*-------------------------------------------------------------------------------
 * Function 	>	Ipark
 * Description 	>	Inverse PARK Transformation
 * Input 		>	fDs
          	   	 	fQs
          	   	 	fTheta
 * Output 		>	fAlpha
 				 	fBeta
 *-------------------------------------------------------------------------------*/
//#pragma CODE_SECTION(Ipark,"ramfuncs");
void Ipark(float32 fDs, float32 fQs, float32 fTheta, float32 *fAlpha, float32 *fBeta)
{
	float32  fSine = sin(fTheta* 6.28318530718F);			// Data: Sine term
	float32  fCosine = cos(fTheta* 6.28318530718F);		// Data: Cosine term

	*fAlpha = (fDs * fCosine) - (fQs * fSine);
	*fBeta  = (fQs * fCosine) + (fDs * fSine);
	return;
}

void Park(float32 fAlpha, float32 fBeta, float32 fTheta, float32 *fDs, float32 *fQs)
{
	float32  fSine = sin(fTheta* 6.28318530718F);			// Data: Sine term
	float32  fCosine = cos(fTheta* 6.28318530718F);		// Data: Cosine term

	*fDs = (fAlpha * fCosine) + (fBeta * fSine);
	*fQs  = (fBeta * fCosine) - (fAlpha * fSine);
	return;
}

/*-------------------------------------------------------------------------------
 * Function 	>	Filter
 * Description 	>	fData with FILTER_DEPTH period average filter in every FILTER_CNT times.
 * Input 		>	v->fDataIn
 * Output 		>	v->fDataOut
 *-------------------------------------------------------------------------------*/
//#pragma CODE_SECTION(Filter,"ramfuncs");
void Filter(struct FILTER *v)
{
	Uint16 uiLoop = 0U;

	for(uiLoop = 0U; uiLoop < FILTER_DEPTH; uiLoop++)
	{
		v->fQueue[uiLoop] = v->fQueue[uiLoop] + v->fDataIn;
	}

	v->uiCnt++;
	if( v->uiCnt == FILTER_CNT )
	{
		v->uiCnt = 0;
		v->fDataOut = v->fQueue[v->uiIndex] / FILTER_DEPTH / FILTER_CNT;
		v->fQueue[v->uiIndex] = 0.f;
		v->uiIndex = (v->uiIndex + 1) % FILTER_DEPTH;
	}
}


//#pragma CODE_SECTION(LowFilter,"ramfuncs");
void LowFilter(struct LOW_FILTER *v)
{
	v->output = v->output_last * v->K2 + v->input * v->K3;
	v->output_last = v->output;
}



/*---------------------------------------------------------------------Dmc-test mode---------------------------------------------------------------------*/
/*-------------------------------------------------------------------------------
 * Function 	>	DmcZeroScan
 * Description 	>	Get the resolver BestOffset. Set uiEn=1 to start DmcZeroScan and Set uiEn=0 to end scan.
 * Input 		>	1.uiEn					2.uiRdcRaw
 * Output 		>	1.sg_uiRslOffsetPos		2.sg_uiRslOffsetNeg		3.uiPercent
 *-------------------------------------------------------------------------------*/
Uint16 DmcZeroScan(struct DMC_DATA *v)
{
	Uint16 uiStep = 0;
	Uint16 uiMaxCnt = 0;
	static struct
	{
		Uint16 			uiOffset;
		Uint16 			uiCnt;
		float32 		fSpdMax;
		Uint16 			uiDir;
		Uint16 			uiRslOffsetPos;
		Uint16 			uiRslOffsetNeg;
		Uint16 			uiPercent;
	}s1 = {0U, 0U, 0.F, 0U, 0U, 0U, 0U};

	if(v->uiEn == 1)
	{
		v->fUthe = ( (v->uiRdcRaw % sg_lK2) * sg_uiK3 + s1.uiOffset ) / (float32)RDC_SCALER;

		v->fSpdFb = SpeedFeed(v->uiRdcRaw);

		SPWMZ_Gen(v->fUamp , v->fUthe , &(v->fTa) , &(v->fTb) , &(v->fTc));

		if(s1.uiDir == 0)
		{
			v->fUamp = 0.2F;
			if(s1.uiOffset < RDC_SCALER)
			{
				uiMaxCnt = (v->fSpdFb < 0.1F) ? F_PWM : (2 * F_PWM);//set wait times
				s1.uiCnt++;
				if(s1.uiCnt > uiMaxCnt)
				{
					s1.uiCnt = 0;
					if(v->fSpdFb > s1.fSpdMax)
					{
						s1.uiRslOffsetPos = s1.uiOffset;
						s1.fSpdMax = v->fSpdFb;
					}
					uiStep = (v->fSpdFb < 0.1F) ? (RDC_SCALER>>4) : (RDC_SCALER>>7);//set step length
					s1.uiOffset = s1.uiOffset + uiStep;
					s1.uiPercent = (Uint32)(s1.uiOffset) * 50 / RDC_SCALER;
				}
			}
			else
			{
				s1.uiDir = 1U;
				s1.uiOffset = 0U;
				s1.uiCnt = 0U;
				s1.fSpdMax = 0.F;
			}
		}
		else if(s1.uiDir == 1U)
		{
			if(s1.uiOffset < RDC_SCALER)
			{
				v->fUamp = -0.2F;
				uiMaxCnt = ( v->fSpdFb > -0.1F) ? F_PWM : (2 * F_PWM);//set wait times
				s1.uiCnt++;
				if(s1.uiCnt > uiMaxCnt)
				{
					s1.uiCnt = 0;
					if(v->fSpdFb < s1.fSpdMax)
					{
						s1.uiRslOffsetNeg = s1.uiOffset;
						s1.fSpdMax = v->fSpdFb;
					}
					uiStep = (v->fSpdFb > -0.1F) ? (RDC_SCALER>>4) : (RDC_SCALER>>7);//set step length
					s1.uiOffset = s1.uiOffset + uiStep;
					s1.uiPercent = (Uint32)(s1.uiOffset) * 50 / RDC_SCALER + 50;
				}
			}
			else
			{
				s1.uiDir = 2U;
				v->fUamp = 0.F;
			}
		}
		else
		{
			sg_uiRslOffsetPos = s1.uiRslOffsetPos;
			sg_uiRslOffsetNeg = s1.uiRslOffsetNeg;
			s1.uiDir = 3U;
			s1.uiPercent = 100U;
		}
	}
	else
	{
		memset(&s1, 0, sizeof(s1));
		v->fUamp = 0.F;
	}

	return s1.uiPercent;
}


Uint16 DmcGetOffset(struct DMC_DATA *v)
{
	float32 fThe90 = 0.F;
	static struct
	{
		Uint16 	uiOffset;
		Uint16 	uiCnt;
		Uint16 	uiErr;
		Uint16	uiDone;
	}s2 = {0U, 0U, 0U, 0U};

	if(v->uiEn == 1)
	{
		if(s2.uiCnt == 0)
		{
			v->fUthe = 0.F;
			v->fUamp = 0.2F;
		}
		else if(s2.uiCnt == 10000)
		{
			s2.uiOffset = RDC_SCALER - ( (v->uiRdcRaw % sg_lK2) * sg_uiK3 );
			v->fUthe = 0.25F;
			v->fUamp = 0.2F;
		}
		else if(s2.uiCnt == 20000)
		{
			fThe90 = ( (v->uiRdcRaw % sg_lK2) * sg_uiK3 + s2.uiOffset) % RDC_SCALER / (float32)RDC_SCALER;
			s2.uiErr = (fThe90 > 0.5F) ? 1U : 0U;
			v->fUthe = 0.F;
			v->fUamp = 0.F;
		}
		else if(s2.uiCnt > 20000)
		{
			s2.uiCnt = 20000U;
			s2.uiDone = 1U;
			sg_uiRslOffsetPos = s2.uiOffset + RDC_SCALER / 4U;
			sg_uiRslOffsetNeg = sg_uiRslOffsetPos;
		}
		s2.uiCnt++;

		SPWMZ_Gen(v->fUamp , v->fUthe , &(v->fTa) , &(v->fTb) , &(v->fTc));
	}
	else
	{
		memset(&s2, 0, sizeof(s2));
	}
	return s2.uiDone;
}

void DmcVfTest(struct DMC_DATA* v)
{
	v->fSpdRefRamp = SpeedRamp(v->uiEn, v->fSpdRef, v->fSpdRefRamp);

	//the = the + delta
	v->fUthe = v->fUthe + v->fSpdRefRamp * sg_fK6;
	v->fUthe = (v->fUthe > 1.F) ? (v->fUthe - 1.F) : v->fUthe;
	v->fUthe = (v->fUthe < -1.F) ? (v->fUthe + 1.F) : v->fUthe;

	SPWMZ_Gen(v->fUamp, v->fUthe, &(v->fTa), &(v->fTb), &(v->fTc));
	return;
}

void DmcVfCtl(struct DMC_DATA* v)
{
	v->fSpdRefRamp = SpeedRamp(v->uiEn, v->fSpdRef, v->fSpdRefRamp);

	//U = U0 + k * speed
	v->fUamp = (v->fSpdRefRamp > 0) ? VF_U0 : -VF_U0;
	v->fUamp = v->fSpdRefRamp * (1.F - v->fUamp) + v->fUamp;
	v->fUamp = LimitF(v->fUamp, 1.F, -1.F);

	//the = the + delta
	v->fUthe = v->fUthe + v->fSpdRefRamp * sg_fK6;
	v->fUthe = (v->fUthe > 1.F) ? (v->fUthe - 1.F) : v->fUthe;
	v->fUthe = (v->fUthe < -1.F) ? (v->fUthe + 1.F) : v->fUthe;

	SPWMZ_Gen(v->fUamp, v->fUthe, &(v->fTa), &(v->fTb), &(v->fTc));
	return;
}

void DmcOpenLoop(struct DMC_DATA* v)
{
	v->fSpdFb = SpeedFeed(v->uiRdcRaw);
	v->fUthe = GetThetaE(v->uiRdcRaw, v->fUamp);

	SPWMZ_Gen(v->fUamp, v->fUthe, &(v->fTa), &(v->fTb), &(v->fTc));
	return;
}

/*-------------------------------------------------------------------------------
 * Function 	>	DmcTestMode()
 * Description 	>
 * Input 		>
 * Output 		>
 *-------------------------------------------------------------------------------*/
void DmcTestMode(struct DMC_DATA *v , Uint16 uiMode)
{
	switch(uiMode)
	{
		case MODE_CLOSE_LOOP:
		{
			DmcSpeedCL(v);
			break;
		}
		case MODE_OPEN_LOOP:
		{
			DmcOpenLoop(v);
			break;
		}
		case MODE_VF_CTL:
		{
			DmcVfCtl(v);
			break;
		}
		case MODE_VF_TEST:
		{
			DmcVfTest(v);
			break;
		}
		case MODE_GET_OFFSET:
		{
			DmcGetOffset(v);
			break;
		}
		case MODE_ZERO_SCAN:
		{
			DmcZeroScan(v);
			break;
		}
		case MODE_THETA_CTL:
		{
			SPWMZ_Gen(v->fUamp, v->fUthe, &(v->fTa), &(v->fTb), &(v->fTc));
			break;
		}
		default:
		{
			v->fUamp = 0.F;
			break;
		}
	}

	return;
}

